#include "servo.h"
#define SERVO_ZOOM (float)0.01
// 168MHZ主频cpu，预分频器1680-1 自动重装载2000-1

PID_x_y_tar PID_servo = {0};

float set_servox_angle(float angle)
{
    uint32_t compare_value = 50 + (uint32_t)((angle / 270.0f) * 200.0f);
    // __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, compare_value);
    return compare_value;
}

float set_servoy_angle(float angle)
{
    // if (angle > 135+90) angle = 135+90;
    // else if (angle < 80) angle = 80;
    uint32_t compare_value = 50 + (uint32_t)((angle / 270.0f) * 200.0f);
    // __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, compare_value);
    return compare_value;
}

void x_set_compare(uint32_t compare)
{
    // __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, compare);
}

void y_set_compare(uint32_t compare)
{
    // //限幅
    // if (compare > 200) compare = 200;
    // else if (compare < 100) compare = 100;
    // __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, compare);
}

void servo_init(PID_x_y_tar *PID_x_y_tar)
{
    // x_set_compare(150);
    // y_set_compare(150);
    // //结构体初始化
    // PID_x_y_tar -> x_compare = 150;
    // PID_x_y_tar -> y_compare = 150;
    // PID_x_y_tar -> x_compare_delta = 0;
    // PID_x_y_tar -> y_compare_delta = 0;
}

// angle_y越大，摄像头越往下
// angle_x越大，摄像头越往右

void find_face_proc(uint16_t time)
{
    // static uint32_t last_time = 0;
    // if (uwTick - last_time > time && cam_data.delta_is_fresh == 1)
    // {
    //     last_time = uwTick;
    //     //获取PID控制值
    //     float x = PID_Calc(&PID2, 0, cam_data.delta_x,0);
    //     printf ("delta_x = %f\n", x);
    //     // printf ("cam_data.delta_x = %f\n", cam_data.delta_x);
    //     float y = PID_Calc(&PID3, 0, cam_data.delta_y,0);
    //     PID_servo.x_compare += (x * SERVO_ZOOM);
    //     PID_servo.y_compare -= (y * SERVO_ZOOM);
    //     printf ("y = %f\n", y);
    //     x_set_compare(PID_servo.x_compare);
    //     printf ("x_compare = %d\n", PID_servo.x_compare);
    //      y_set_compare(PID_servo.y_compare);
    //     x = 0;
    //     y = 0;
    //     cam_data.delta_is_fresh = 0;
    // }
}

void servo1_init(void)
{
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 15); // 初始化为90度位置(1.5ms)
}

void servo1_ctl(uint8_t cmpval)  
{
    if (cmpval > 25)
        cmpval = 25; // 限制最大值(2.5ms)
    else if (cmpval < 5)
        cmpval = 5; // 限制最小值(0.5ms)
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, cmpval);
}

// 角度控制函数（推荐使用）
void servo1_set_angle(float angle)
{
    if (angle > 180.0f)
        angle = 180.0f;
    else if (angle < 0.0f)
        angle = 0.0f;

    // 将角度转换为比较值
    // 0度 -> 5 (0.5ms), 90度 -> 15 (1.5ms), 180度 -> 25 (2.5ms)
    uint8_t cmp_val = 5 + (uint8_t)((angle / 180.0f) * 20.0f);
    servo1_ctl(cmp_val);
}

void servo_proc(void)
{
    static uint8_t cnt = 0;


    if (cnt >= 18)
    {
        cnt = 0;
    }
    else
    {
        cnt++;
    }

    // 从0度到180度循环
    servo1_set_angle(cnt * 10.0f); // 0, 10, 20, ..., 180度
}
